// -*- Mode: Java -*-
Integer Lookup missionStart;
Integer Lookup armStatus;
Integer Lookup takeoffStatus;
Integer Lookup numMissionWP;
Integer Lookup nextMissionWPIndex;
Real[3] Lookup position;
Real[3] Lookup velocity;

// Low level Autopilot commands
Command SetMode(String mode);
Command SetYaw(Real heading);
Command Takeoff(Real altitude);
Command ArmMotors();
Command Land();
Command SetNextMissionWP(Integer index);
Command Status(String text);
Command pprint(...);
Command SetVel(Real velocity[3]);
Command SetPos(Real position[3]);

ARDUPILOT_TEST:
{

    Real _Position[3];
    Real _Velocity[3];
        	        
    FMS:
    {
        Boolean _idleStatus    = true;
        Boolean _takeoffStatus = false;
        Boolean _climbStatus   = false;
        Boolean _cruiseStatus  = false;
        Boolean _landStatus    = false;
        Boolean _ditchStatus   = false;
        Integer _numMissionWP;
        Integer _nextMissionWP = 0;

        Repeat true;

        IDLE:
        {
            Integer start;
            StartCondition _idleStatus;
            EndCondition !_idleStatus;

            {
                Repeat true;
                start = Lookup(missionStart);

                if (start == 0){
                    _numMissionWP = Lookup(numMissionWP);
                    Status("IC:Starting takeoff");
                    _takeoffStatus = true;
                    _idleStatus = false;
                }elseif(start > 0){
                    _numMissionWP = Lookup(numMissionWP);
                    Status("IC:Starting cruise");
                    _cruiseStatus = true;
                    _idleStatus = false;
                }endif
            }

        }// End of IDLE

        TAKEOFF:{
            Integer _armStatus;
            StartCondition _takeoffStatus;
            SkipCondition !_takeoffStatus;
            EndCondition !_takeoffStatus;

            pprint("Plexil status: Starting takeoff");

            CHANGE_MODE:SetMode("ACTIVE");

            ARM:{
                EndCondition isKnown(_armStatus) && (_armStatus >= 0);

                ArmMotors();

                CHECK_STATUS: {
                    Repeat true;
                    _armStatus = Lookup(armStatus);
                }
            }// End of ARM

            THROTTLE_UP:{
                Integer status;
                StartCondition (_armStatus==1);
                SkipCondition  (_armStatus!=1);


                TAKEOFF:{

                    EndCondition isKnown(status) && (status>=0);

                    Takeoff(5.0);

                    CHECK_STATUS: {
                        Repeat true;
                        status = Lookup(takeoffStatus);
                    }

                }

                if(status == 1){
                    Status("IC:Starting climb");
                    _climbStatus = true;
                    _takeoffStatus = false;
                }else{
                    pprint("Plexil status: Takeoff failed\n");
                    _idleStatus = true;
                    _takeoffStatus = false;
                }endif

            }// End of THROTTLE
        }// End of TAKEOFF


        CLIMB:{
            StartCondition _climbStatus;
            SkipCondition !_climbStatus;
            EndCondition !_climbStatus;
            {
                Boolean val;
                Repeat true;

                _Position = Lookup(position);

                if(_Position[2] >= 3.0){
                    pprint("Plexil status: Starting Cruise");
                    SetNextMissionWP(_nextMissionWP);
                    SetMode("PASSIVE");
                    Status("IC:Starting cruise");
                    _cruiseStatus = true;
                    _climbStatus = false;
                }endif
            }

        }// End of CLIMB

        CRUISE:{

            Integer nextWP;
            StartCondition _cruiseStatus;
            SkipCondition !_cruiseStatus;
            Repeat true;

            nextWP = Lookup(nextMissionWPIndex);

            if(nextWP >=  3){

                 {
                   Integer i=0;
                   Real gotopos[3];

                   gotopos[0] = 0;
                   gotopos[1] = 0;
                   gotopos[2] = 0;

                   SetMode("ACTIVE");


                   SetPos(gotopos);

                   while(i<100000){
                      i = i + 1;
                   }

                 }


                 _cruiseStatus = false;
                 _landStatus = true;
            }endif

        }// End of CRUISE

        LAND:{
            StartCondition _landStatus;
            SkipCondition !_landStatus;

            pprint("Plexil status: Landing");
            SetMode("ACTIVE");
            Land();

            _landStatus = false;
            _idleStatus = true;
        }// End of LAND

    }// End of FMS
}// End of ICAROUS

